#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：刘骏轩2025讯飞
@File ：picturePub.py
@Author ：ljx
@Date ：2025/7/26 下午8:45 
'''

import sys
sys.path.insert(0, '/home/ucar/cv_bridge_ws/devel/lib/python3/dist-packages')
import cv2
import numpy as np
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import time
from VideoOperator import videoOperator

if __name__ == "__main__":
    cap = videoOperator()
    rospy.init_node('Camera', anonymous=True)  # 定义节点
    Publisher = rospy.Publisher('/picture', Image, queue_size=2)  # 定义话题
    cv_bridge = CvBridge()
    rate = rospy.Rate(35)

    while not rospy.is_shutdown():
        # start = time.time()
        frame = cap.Get_Image()
        if frame is None:
            rate.sleep()
            continue
        ros_frame =cv_bridge.cv2_to_imgmsg(frame, encoding="bgr8")
        ros_frame.header.stamp = rospy.Time.now()
        ros_frame.header.frame_id = "Camera"

        #rospy.loginfo("Camera Publish Picture Successfully")

        Publisher.publish(ros_frame)

        # end = time.time()
        # print("cost time:", end - start)  # 看一下每一帧的执行时间，从而确定合适的rate
        rate.sleep()
    cv2.destroyAllWindows()
